#####################
# Bioloid Contol Interface
# Written in Python 2.6
# Gui in Wxpython generated using WxGlade
# Oct 2009
# Martin Mason
############################

from CRC import *
import wx
import time
from math import copysign
import serial
from BioloidHighLevelCmds import *

#Limits on parameters for Joystick control to prevent hard falls.
#There are linear ramps through the parameter space, but they are probably to
#fast.  This could be controlled by changing the joystick polling.
#All the joystick code needs clean up but 2AM

min_stridelength = -7
min_frequency = 16

max_swing = 4
stride_mutliplier = 1.8
max_strideheight = 7
max_stridelength = 12
max_frequency = 24
#SH, SL, Sw,SOX,SOY,Tpp,Tilt,Turn,Freq,Camber
Initial_Values = [0,0,0,0,20,2,60,0,16,12]

forward_value = 0

Turning = False
serial_wait = 0.01


#########################################################################
# Create the main frame Window
########################################################################
class JoystickHandler(wx.Frame):
    def __init__(self, *args, **kwds):

        # begin wxGlade: MainFrame.__init__
        kwds["style"] = wx.DEFAULT_FRAME_STYLE
        #Create the Frame
        wx.Frame.__init__(self, *args, **kwds)

        #Create the Joystick Callback timer with a delay of 100ms
        self.Joystick = wx.Joystick()
        self.Joystick.SetCapture(self)
        print self.Joystick.GetProductName()
        print "# Joysticks: ", self.Joystick.GetNumberJoysticks()
        stick = []
        sticklist = []
        if self.Joystick.GetNumberJoysticks() > 0:
            for i in range(0,self.Joystick.GetNumberJoysticks()):
                stick.append(wx.Joystick(i))
                sticklist.append(str(i)+": "+stick[i].GetProductName())

        #Create the Status Bar
        self.frame_1_statusbar = self.CreateStatusBar(1, 0)
        self.Joystick_List = wx.ListBox(self, -1, choices=sticklist)
        self.Connect_Joystick = wx.Button(self, -1, "Connect Joystick")
        self.Connect_Joystick.Bind(wx.EVT_BUTTON, self.Start_Joystick)
        
        #Create the text on the top bar
        #self.bitmap_1 = wx.StaticBitmap(self, -1, wx.Bitmap("C:\\Documents and Settings\\Administrator\\My Documents\\Robot_Evolution_small.jpg", wx.BITMAP_TYPE_ANY))
        self.label_8 = wx.StaticText(self, -1, "University of Plymouth\nRobot Control Interface", style=wx.ALIGN_CENTRE)
        self.Stop = wx.Button(self, -1, "Stop")
        self.Stop.Bind(wx.EVT_BUTTON, self.stop)
        self.Exit = wx.Button(self, -1, "Exit")
        self.Exit.Bind(wx.EVT_BUTTON, self.terminate_program)
        #self.RampCurrent = wx.Button(self, -1, "Ramp from Last")
        #Setup the labels and sliders to control the parameters
        self.label_2 = wx.StaticText(self, -1, "Stride Length")
        self.label_3 = wx.StaticText(self, -1, "Stride Height")
        self.label_4 = wx.StaticText(self, -1, "Swing")
        self.label_5 = wx.StaticText(self, -1, "Stride Offset Y")
        self.label_6 = wx.StaticText(self, -1, "Frequency")
        self.label_7 = wx.StaticText(self, -1, "Camber")
        #Set the EventId to the value that will be passed to the Serial Port 
        self.Stride_Length_Slider = wx.Slider(self, 0, 0, -30, 30, style=wx.SL_HORIZONTAL|wx.SL_AUTOTICKS|wx.SL_LABELS)
        self.Stride_Height_Slider = wx.Slider(self, 1, 0, -20, 20, style=wx.SL_HORIZONTAL|wx.SL_AUTOTICKS|wx.SL_LABELS)
        self.Swing_Slider = wx.Slider(self, 2, 0, -10, 10, style=wx.SL_HORIZONTAL|wx.SL_AUTOTICKS|wx.SL_LABELS)
        self.Body_Offset_Slider = wx.Slider(self, 4, 0, 0, 69, style=wx.SL_HORIZONTAL|wx.SL_AUTOTICKS|wx.SL_LABELS)
        self.Frequency_Slider = wx.Slider(self, 8, 32, 0, 50, style=wx.SL_HORIZONTAL|wx.SL_AUTOTICKS|wx.SL_LABELS)
        self.Camber_Slider = wx.Slider(self, 9, 0, -30, 30, style=wx.SL_HORIZONTAL|wx.SL_AUTOTICKS|wx.SL_LABELS)
        self.label_9 = wx.StaticText(self, -1, "Tilt")
        self.label_10 = wx.StaticText(self, -1, "Stride Offset X")
        self.label_11 = wx.StaticText(self, -1, "Ticks per Pose")
        self.label_12 = wx.StaticText(self, -1, "Turn")
        self.label_13 = wx.StaticText(self, -1, "Right Arm")
        self.label_14 = wx.StaticText(self, -1, "Left Arm")
        self.Tilt_Slider = wx.Slider(self, 6, 0, -100, 100, style=wx.SL_HORIZONTAL|wx.SL_AUTOTICKS|wx.SL_LABELS)
        self.Stride_Offset_x_Slider = wx.Slider(self, 3, 0, -30, 30, style=wx.SL_HORIZONTAL|wx.SL_AUTOTICKS|wx.SL_LABELS)
        self.Ticks_per_Pose_Slider = wx.Slider(self, 5, 0, 0, 6, style=wx.SL_HORIZONTAL|wx.SL_AUTOTICKS|wx.SL_LABELS)
        self.Turn_Slider = wx.Slider(self, 7, 0, -30, 30, style=wx.SL_HORIZONTAL|wx.SL_AUTOTICKS|wx.SL_LABELS)
        self.Right_Arm_Slider = wx.Slider(self, 10, 0, -30, 30, style=wx.SL_HORIZONTAL|wx.SL_AUTOTICKS|wx.SL_LABELS)
        self.Left_Arm_Slider = wx.Slider(self, 11, 0, -30, 30, style=wx.SL_HORIZONTAL|wx.SL_AUTOTICKS|wx.SL_LABELS)

        #Create the box to display what is sent out
        self.text_ctrl = wx.TextCtrl(self, -1, "")

        #Bind Sliders to appropriate functions
        self.Right_Arm_Slider.Bind(wx.EVT_SCROLL,self.OnScroll)
        self.Left_Arm_Slider.Bind(wx.EVT_SCROLL,self.OnScroll)
        self.Stride_Length_Slider.Bind(wx.EVT_SCROLL,self.OnScroll)
        self.Stride_Height_Slider.Bind(wx.EVT_SCROLL,self.OnScroll)
        self.Swing_Slider.Bind(wx.EVT_SCROLL,self.OnScroll)
        self.Body_Offset_Slider.Bind(wx.EVT_SCROLL,self.OnScroll)
        self.Frequency_Slider.Bind(wx.EVT_SCROLL,self.OnScroll)
        self.Camber_Slider.Bind(wx.EVT_SCROLL,self.OnScroll)
        self.Tilt_Slider.Bind(wx.EVT_SCROLL,self.OnScroll)
        self.Stride_Offset_x_Slider.Bind(wx.EVT_SCROLL,self.OnScroll)
        self.Ticks_per_Pose_Slider.Bind(wx.EVT_SCROLL,self.OnScroll)
        self.Turn_Slider.Bind(wx.EVT_SCROLL,self.OnScroll)
        
        self.__set_properties()
        self.__do_layout()
        # end wxGlade

    def __set_properties(self):
        # begin wxGlade: MainFrame.__set_properties
        self.SetTitle("frame_1")
        self.frame_1_statusbar.SetStatusWidths([-1])
        # statusbar fields
        frame_1_statusbar_fields = ["Version 0.1 Oct 3 2009"]
        for i in range(len(frame_1_statusbar_fields)):
            self.frame_1_statusbar.SetStatusText(frame_1_statusbar_fields[i], i)
        self.label_8.SetFont(wx.Font(16, wx.SWISS, wx.NORMAL, wx.BOLD, 0, ""))
        # end wxGlade

    def __do_layout(self):
        # begin wxGlade: MainFrame.__do_layout
        sizer_1 = wx.BoxSizer(wx.VERTICAL)
        sizer_2 = wx.BoxSizer(wx.VERTICAL)
        grid_sizer_1 = wx.GridSizer(4, 6, 2, 5)
        sizer_3 = wx.BoxSizer(wx.HORIZONTAL)
        sizer_6 = wx.BoxSizer(wx.VERTICAL)
        sizer_5 = wx.BoxSizer(wx.HORIZONTAL)
        #sizer_3.Add(self.bitmap_1, 0, 0, 0)
        sizer_3.Add(self.label_8, 0, wx.ALIGN_CENTER_HORIZONTAL|wx.ALIGN_CENTER_VERTICAL, 0)
        sizer_3.Add(sizer_5, 1, 0, 0)
        sizer_3.Add(sizer_6, 1, 0, 0)
        sizer_6.Add(self.Stop, 0, wx.ALIGN_CENTER_HORIZONTAL, 0)
        sizer_6.Add(self.Exit, 0, wx.ALIGN_CENTER_HORIZONTAL, 0)
        #sizer_6.Add(self.RampCurrent, 0, wx.ALIGN_CENTER_HORIZONTAL, 0)
        sizer_5.Add(self.Joystick_List, 0, 0, 0)
        sizer_5.Add(self.Connect_Joystick, 0, wx.ALIGN_CENTER_HORIZONTAL, 0)
        sizer_2.Add(sizer_3, 0, 0, 0)
        grid_sizer_1.Add(self.label_2, 0, wx.ALIGN_BOTTOM|wx.ALIGN_CENTER_HORIZONTAL, 0)
        grid_sizer_1.Add(self.label_3, 0, wx.ALIGN_BOTTOM|wx.ALIGN_CENTER_HORIZONTAL, 0)
        grid_sizer_1.Add(self.label_4, 0, wx.ALIGN_BOTTOM|wx.ALIGN_CENTER_HORIZONTAL, 0)
        grid_sizer_1.Add(self.label_5, 0, wx.ALIGN_BOTTOM|wx.ALIGN_CENTER_HORIZONTAL, 0)
        grid_sizer_1.Add(self.label_6, 0, wx.ALIGN_BOTTOM|wx.ALIGN_CENTER_HORIZONTAL, 0)
        grid_sizer_1.Add(self.label_7, 0, wx.ALIGN_BOTTOM|wx.ALIGN_CENTER_HORIZONTAL, 0)
        grid_sizer_1.Add(self.Stride_Length_Slider, 0, wx.EXPAND|wx.ALIGN_CENTER_HORIZONTAL, 0)
        grid_sizer_1.Add(self.Stride_Height_Slider, 0, wx.EXPAND|wx.ALIGN_CENTER_HORIZONTAL, 0)
        grid_sizer_1.Add(self.Swing_Slider, 0, wx.EXPAND|wx.ALIGN_CENTER_HORIZONTAL, 0)
        grid_sizer_1.Add(self.Body_Offset_Slider, 0, wx.EXPAND|wx.ALIGN_CENTER_HORIZONTAL, 0)
        grid_sizer_1.Add(self.Frequency_Slider, 0, wx.EXPAND|wx.ALIGN_CENTER_HORIZONTAL, 0)
        grid_sizer_1.Add(self.Camber_Slider, 0, wx.EXPAND|wx.ALIGN_CENTER_HORIZONTAL, 0)
        grid_sizer_1.Add(self.label_9, 0, wx.ALIGN_BOTTOM|wx.ALIGN_CENTER_HORIZONTAL, 0)
        grid_sizer_1.Add(self.label_10, 0, wx.ALIGN_BOTTOM|wx.ALIGN_CENTER_HORIZONTAL, 0)
        grid_sizer_1.Add(self.label_11, 0, wx.ALIGN_BOTTOM|wx.ALIGN_CENTER_HORIZONTAL, 0)
        grid_sizer_1.Add(self.label_12, 0, wx.ALIGN_BOTTOM|wx.ALIGN_CENTER_HORIZONTAL, 0)
        grid_sizer_1.Add(self.label_13, 0, wx.ALIGN_BOTTOM|wx.ALIGN_CENTER_HORIZONTAL, 0)
        grid_sizer_1.Add(self.label_14, 0, wx.ALIGN_BOTTOM|wx.ALIGN_CENTER_HORIZONTAL, 0)
        grid_sizer_1.Add(self.Tilt_Slider, 0, wx.EXPAND|wx.ALIGN_CENTER_HORIZONTAL, 0)
        grid_sizer_1.Add(self.Stride_Offset_x_Slider, 0, wx.EXPAND|wx.ALIGN_CENTER_HORIZONTAL, 0)
        grid_sizer_1.Add(self.Ticks_per_Pose_Slider, 0, wx.EXPAND|wx.ALIGN_CENTER_HORIZONTAL, 0)
        grid_sizer_1.Add(self.Turn_Slider, 0, wx.EXPAND|wx.ALIGN_CENTER_HORIZONTAL, 0)
        grid_sizer_1.Add(self.Right_Arm_Slider, 0, wx.EXPAND|wx.ALIGN_CENTER_HORIZONTAL, 0)
        grid_sizer_1.Add(self.Left_Arm_Slider, 0, wx.EXPAND|wx.ALIGN_CENTER_HORIZONTAL, 0)
        sizer_2.Add(grid_sizer_1, 1, wx.EXPAND, 0)
        sizer_2.Add(self.text_ctrl, 0, wx.EXPAND, 0)
        sizer_1.Add(sizer_2, 1, wx.EXPAND, 0)
        self.SetSizer(sizer_1)
        sizer_1.Fit(self)
        self.Layout()
        # end wxGlade

            
    def OnScroll(self, event):
        # position is an int value
        temp_value = event.GetPosition()
        status = setGait(event.GetId(),temp_value)
        time.sleep(serial_wait)
        self.text_ctrl.WriteText(status + chr(13))

    def SendCommand(self, slider_number,temp_value): #Cheesy hack.  Really should just have one 
        # position is an int value
        status = setGait(slider_number,temp_value)
        self.text_ctrl.WriteText(status + chr(13))
        time.sleep(serial_wait)

    def stop(self,event):
        global forward_value
        print "Stopping"
        self.SendCommand(0,0) #Stride Length to       
        self.SendCommand(8,20) #Frequency to 20        
        self.SendCommand(1,6) #Stride Height to 6        
        self.SendCommand(2,3) #Ramp Swing to 0        
        self.SendCommand(9,12) #Camber to 12        
        for i in range(3,-1,-1):
            self.SendCommand(2,i) #Swing
        self.SendCommand(1,0)        
        self.SendCommand(8,max_frequency)
        self.Frequency_Slider.SetValue(max_frequency)
        self.Stride_Length_Slider.SetValue(0)
        self.Swing_Slider.SetValue(0)
        self.Turn_Slider.SetValue(0)
        self.Stride_Height_Slider.SetValue(0)
        self.Stride_Length_Slider.SetValue(0)
        forward_value=0
        
    def Set_Initial_Values(self):
        global Initial_Values, forward_value
        print "initialize"
        
        sliderlist = [self.Stride_Height_Slider,self.Stride_Length_Slider,self.Swing_Slider,self.Stride_Offset_x_Slider,self.Body_Offset_Slider, 
                      self.Ticks_per_Pose_Slider,self.Tilt_Slider,self.Turn_Slider,self.Frequency_Slider,self.Camber_Slider]
        for i in range(0,10):
            self.SendCommand(i,Initial_Values[i])
            sliderlist[i].SetValue(Initial_Values[i])
        forward_value=0
        

    def turnleft(self):
        global Turning
        #self.Turn_Slider.SetValue(self.Turn_Slider.GetValue()+2)
        self.Turn_Slider.SetValue(15)
        self.SendCommand(7,self.Turn_Slider.GetValue())
        Turning = True
        if forward_value ==0:
            self.Swing_Slider.SetValue(2)
            self.SendCommand(2,self.Swing_Slider.GetValue())
            self.Stride_Height_Slider.SetValue(4)
            self.SendCommand(1,self.Stride_Height_Slider.GetValue())
            self.Frequency_Slider.SetValue(28)
            self.SendCommand(8,self.Frequency_Slider.GetValue())
            self.Stride_Length_Slider.SetValue(2)
            self.SendCommand(0,self.Stride_Length_Slider.GetValue())

    def turnright(self):
        global Turning
        Turning = True
        self.Turn_Slider.SetValue(-15)
        #self.Turn_Slider.SetValue(self.Turn_Slider.GetValue()-2)
        self.SendCommand(7,self.Turn_Slider.GetValue())
        self.Swing_Slider.SetValue(2)
        if forward_value == 0: 
            self.SendCommand(2,self.Swing_Slider.GetValue())
            self.Stride_Height_Slider.SetValue(4)
            self.SendCommand(1,self.Stride_Height_Slider.GetValue())
            self.Frequency_Slider.SetValue(28)
            self.SendCommand(8,self.Frequency_Slider.GetValue())
            self.Stride_Length_Slider.SetValue(2)
            self.SendCommand(0,self.Stride_Length_Slider.GetValue())

    def go(self):
        global forward_value
        self.Turn_Slider.SetValue(0)
        self.SendCommand(7,0)
        #####
        # New system based on Forward
        ######
        if forward_value in range(0,max_swing+1) or forward_value in range(-max_swing,0):
            self.Tilt_Slider.SetValue(Initial_Values[5] - forward_value)
            self.SendCommand(2,self.Tilt_Slider.GetValue())
            self.Swing_Slider.SetValue(abs(forward_value))
            self.SendCommand(2,self.Swing_Slider.GetValue())
            self.Stride_Height_Slider.SetValue(int(abs(forward_value)*stride_mutliplier))
            self.SendCommand(1,self.Stride_Height_Slider.GetValue())
            self.Frequency_Slider.SetValue((min_frequency+2*abs(forward_value)))
            self.SendCommand(8,self.Frequency_Slider.GetValue())
        if forward_value in range(max_swing+1,max_swing+max_stridelength) or forward_value in range(min_stridelength-max_swing,-max_swing):
            self.Stride_Length_Slider.SetValue(copysign(1,forward_value)*(abs(forward_value)-max_swing-1))
            self.SendCommand(0,self.Stride_Length_Slider.GetValue())

    #This function is polled continually by the timer to read the joystick status
            
    def CheckJoystick(self):
        global forward_value, Turning
        ButtonState = self.Joystick.GetButtonState() 
        for ButtonIndex in range(8): #Buttons are bits so only 16 supported
            Flag = 1 << ButtonIndex
            if (ButtonState & Flag) and not (self.OldJoystickButtonState & Flag):
                #print ButtonIndex
                if ButtonIndex == 1:  #Stop Button
                    self.stop(self)
                if ButtonIndex == 7:  #Set Initial Values Button
                    self.Set_Initial_Values()
        self.OldJoystickButtonState = ButtonState
        # Check for joystick directions:
        #For Simple Joystick
        #X = self.Joystick.GetPosition().x
        #For Xbox controller
        X = self.Joystick.GetUPosition()
        Y = self.Joystick.GetPosition().y
        #For XBox Controller


        if (X < 500 and X > -500):
            print "left"
            self.turnleft()
        elif (X > 60000):
            print "right"
            self.turnright()
        elif (Y < 500 and Y > -500):
            self.Turn_Slider.SetValue(0)
        #self.Turn_Slider.SetValue(self.Turn_Slider.GetValue()-2)
            self.SendCommand(7,self.Turn_Slider.GetValue())
            print "up"
            if forward_value in range(min_stridelength-max_swing,max_swing+max_stridelength):
                forward_value += 1
                self.go()

        elif (Y > 60000):
            print "down"
            if forward_value in range(min_stridelength-max_swing,max_swing+max_stridelength):
                forward_value -= 1
                self.go()

        else:
            #print forward_value
            if forward_value >= 1:
                forward_value -= 1
                self.go()

            if forward_value < 0:
                forward_value += 1
                self.go()
            if Turning == True and forward_value == 0:
                Turning = False
                self.Set_Initial_Values()


    def Start_Joystick(self,event ):
        self.Joystick = wx.Joystick(self.Joystick_List.GetSelection())
        self.Joystick.SetCapture(self)
        self.Timer = wx.CallLater(100, self.OnTimer)
        self.Set_Initial_Values()

    def terminate_program(self,event):
        self.stop(self)
        self.Destroy()
        
    #Joystick Timer        
    def OnTimer(self, *args, **kw):
         if self.Joystick != None:
            self.CheckJoystick()
            self.Timer.Restart() # make the timer get called again:
# end of class MainFrame


